// system include.
#include <pthread.h>
#include <thread>
#include <iostream>
#include <fstream>
#include <string.h>
#include <unistd.h> // 引入 usleep()
// apply include.
#include <mutex>

#include "control.pb.h"
#include "localization.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>

#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>

#include "gnssApi.h"

std::string buf = "b'$GNRMC,082215.00,A,3111.15935,N,12034.95566,E,0.008,,171023,,,R,V*01\r\n$GNVTG,,T,,M,0.008,N,0.014,K,D*35\r\n$GNGGA,082215.00,3111.15935,N,12034.95566,E,4,12,0.86,37.3,M,7.8,M,1.0,0000*65\r\n$GNGSA,A,3,26,28,31,32,10,16,29,03,,,,,1.63,0.86,1.39,1*03\r\n$GNGSA,A,3,66,65,72,76,75,77,,,,,,,1.63,0.86,1.39,2*06\r\n$GNGSA,A,3,21,19,04,,,,,,,,,,1.63,0.86,1.39,3*0D\r\n$GNGSA,A,3,36,39,07,06,10,13,08,09,16,,,,1.63,0.86,1.39,4*0E\r\n$GNGSA,A,3,07,02,04,,,,,,,,,,1.63,0.86,1.39,5*05\r\n$GPGSV,3,1,10,03,16,316,29,10,19,183,39,16,18,215,38,26,48,225,46,1*69\r\n$GPGSV,3,2,10,28,59,352,44,29,28,097,40,31,49,305,45,32,69,106,46,1*6E\r\n$GPGSV,3,3,10,41,36,236,41,50,53,168,41,1*6E\r\n$GPGSV,2,1,07,03,16,316,19,10,19,183,36,26,48,225,38,28,59,352,42,6*6B\r\n$GPGSV,2,2,07,29,28,097,31,31,49,305,38,32,69,106,42,6*54\r\n$GPGSV,1,1,02,25,32,043,,40,12,257,,0*61\r\n$GLGSV,2,1,07,65,71,164,46,66,49,329,50,72,16,152,40,75,35,026,20,1*7A\r\n$GLGSV,2,2,07,76,62,292,48,77,24,244,32,,,,38,1*71\r\n$GLGSV,2,1,07,65,71,164,32,66,49,329,39,72,16,152,19,75,35,026,13,3*78\r\n$GLGSV,2,2,07,76,62,292,41,77,24,244,26,,,,22,3*74\r\n$GLGSV,1,1,01,67,00,331,,0*48\r\n$GAGSV,1,1,04,04,11,314,19,07,09,129,40,19,68,335,43,21,88,186,45,2*71\r\n$GAGSV,2,1,05,04,11,314,13,07,09,129,36,19,68,335,39,20,,,37,7*4B\r\n$GAGSV,2,2,05,21,88,186,43,7*4D\r\n$GAGSV,1,1,01,27,35,042,,0*40\r\n$GBGSV,2,1,08,06,59,333,43,07,33,185,38,08,19,207,33,09,51,315,37,1*78\r\n$GBGSV,2,2,08,10,23,194,34,16,62,353,43,36,67,304,43,39,63,010,44,1*7E\r\n$GBGSV,2,1,07,06,59,333,41,07,33,185,40,08,19,207,43,09,51,315,44,B*0A\r\n$GBGSV,2,2,07,10,23,194,38,13,24,218,37,16,62,353,45,B*39\r\n$GBGSV,1,1,04,19,63,190,,20,19,157,,45,22,253,,46,44,039,,0*72\r\n$GQGSV,1,1,04,02,70,108,42,03,09,155,33,04,51,170,44,07,53,168,38,1*63\r\n$GQGSV,1,1,04,02,70,108,41,03,09,155,31,04,51,170,42,07,53,168,40,6*6C\r\n$GNGLL,3111.15935,N,12034.95566,E,082215.00,A,D*7A\r\n'";

enum GNGGA
{
    Time = 1,
    LatitudeRange = 2,
    Latitude  = 3,
    LongitudeRange ,
    Longitude,
    GpsStatus,
    StartNum
};

int getGNGGA(std::string str, S_GNGGA &gnggaData)
{
    uint32_t index = str.find("GNGGA", 0); // 在srt字符串中寻找“GNGGA”字符串的位置
    if(index < str.length()){ // 如果找到了GNGGA语句
        std::vector<std::string> result=split(str,","); // 按照逗号对GNGGA语句进行分割
        // std::cout<<"The result:"<<std::endl; // 输出分割结果
        for(int i=0; i<result.size(); i++) {
            //std::cout<<result[i]<<std::endl;
            switch (i)
            {
            case Time: // 字段1：UTC 时间，hhmmss.sss，时分秒格式
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.time = stod(result[i],0); // 把字符串s从p开始转换成double
                    // printf("字段1 UTC 时间:%f\n", gnggaData.time);
                }              
                break;
            case Latitude: // 字段2：纬度ddmm.mmmm，度分格式（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.latitude =  changeData(stod(result[i],0)); // 把字符串s从p开始转换成double // 再转换成经纬度
                    // printf("字段2 纬度:%lf\n", gnggaData.latitude);
                }    
                break;
            case LatitudeRange: // 字段3：纬度N（北纬）或S（南纬）
                {
                    gnggaData.latitudeRange = result[i]; // 把字符串s从p开始转换成double
                    // std::cout << "字段3 纬度:" << gnggaData.latitudeRange << std::endl;
                }   
                break;
            case Longitude: // 字段4：经度dddmm.mmmm，度分格式（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.longitude = changeData(stod(result[i],0)); // 把字符串s从p开始转换成double // 再转换成经纬度
                    // printf("字段4 经度:%lf\n", gnggaData.longitude);
                } 
                break;
            case LongitudeRange: // 字段5：经度E（东经）或W（西经）
                {
                    gnggaData.longitudeRange = result[i]; // 把字符串s从p开始转换成double
                    // std::cout << "字段5 经度:" << gnggaData.longitudeRange << std::endl;
                }   
                break;
            case GpsStatus: // 字段6：GPS状态，0=未定位，1=非差分定位，2=差分定位，3=无效PPS，6=正在估算
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.gpsStatus = stod(result[i],0); // 把字符串s从p开始转换成double
                    // std::cout << "字段6 GPS状态:" << gnggaData.gpsStatus << std::endl;
                }    
                break;
            case StartNum: // 字段7：正在使用的卫星数量（00 - 12）（前导位数不足则补0）
                if(judgeStrToInt(result[i]) == true){ // 如果字符串全是数字
                    gnggaData.startNum = stod(result[i],0); // 把字符串s从p开始转换成double
                    // std::cout << "字段7 卫星数量:" << gnggaData.startNum << std::endl;
                }      
                break;
            default:{}
                break;
            }
        }
        return true;
    }
    else{
        return false;
    }
    
}


// 原文链接：https://blog.csdn.net/Long_xu/article/details/135306358
int main(int argc, char *argv[])
{
    S_GNGGA gnggaData;
    eCAL::Initialize(argc, argv, "GNGGA Publisher");
    eCAL::protobuf::CPublisher<MowerProtoPakg::localizationGNGGA> ecalGNGGApublisher("GNGGAMessage");

    std::string input_file="/home/firefly/code/mower_drives/test/RTK_original_data.txt";
    std::string query;
    std::ifstream in(input_file); // 按行读取数据
    if(in){
        while(getline(in,query)){
            // std::cout<<query+"\r\n"<<std::endl;
            if(getGNGGA(query, gnggaData) == true){
                spdlog::info("yes UTC:{} 纬度:{}  {} 经度:{}  {} 卫星数量:{}",
                    gnggaData.time, gnggaData.latitude, gnggaData.latitudeRange, gnggaData.longitude, gnggaData.longitudeRange, gnggaData.startNum);

                // 测试定位订阅数据
                // 把数据包装成protobuf
                MowerProtoPakg::localizationGNGGA localizationGNGGAMessage;// 创建一个protobuf消息对象
                localizationGNGGAMessage.set_time(gnggaData.time);
                localizationGNGGAMessage.set_latitude(gnggaData.latitude);
                localizationGNGGAMessage.set_latituderange(gnggaData.latitudeRange);
                localizationGNGGAMessage.set_longitude(gnggaData.longitude);
                localizationGNGGAMessage.set_longituderange(gnggaData.longitudeRange);
                localizationGNGGAMessage.set_gpsstatus(gnggaData.gpsStatus);
                localizationGNGGAMessage.set_startnum(gnggaData.startNum);
                localizationGNGGAMessage.set_precision(0);
                localizationGNGGAMessage.set_altitude(0);
                localizationGNGGAMessage.set_geoidheight(0);
                // Send the message
                ecalGNGGApublisher.Send(localizationGNGGAMessage);
                sleep(1); // 2Hz
            }
        }
    }
    else{
        std::cout<< "not file"<<std::endl;
    }
    eCAL::Finalize();
    return 0;
}

int ts(int argc, char *argv[])
{
    float speed = 0;
    eCAL::Initialize(argc, argv, "GNGGA Publisher");
    eCAL::protobuf::CPublisher<MowerProtoPakg::localizationGNGGA> ecalGNGGApublisher("GNGGAMessage");
    eCAL::protobuf::CPublisher<MowerProtoPakg::controlMoveOrder> ecalcontrolMovepublisher("ControlMessage");
    while (eCAL::Ok()) {
        // 测试定位订阅数据
        // 把数据包装成protobuf
        MowerProtoPakg::localizationGNGGA localizationGNGGAMessage;// 创建一个protobuf消息对象
        localizationGNGGAMessage.set_time(010337.10);
        localizationGNGGAMessage.set_latitude(3040.25368);
        localizationGNGGAMessage.set_latituderange("N");
        localizationGNGGAMessage.set_longitude(10406.42449);
        localizationGNGGAMessage.set_longituderange("E");
        localizationGNGGAMessage.set_gpsstatus(4);
        localizationGNGGAMessage.set_startnum(12);
        localizationGNGGAMessage.set_precision(1);
        localizationGNGGAMessage.set_altitude(18);
        localizationGNGGAMessage.set_geoidheight(587.7);
        // Send the message
        ecalGNGGApublisher.Send(localizationGNGGAMessage);

        sleep(0.5); // 2Hz
        //std::this_thread::sleep_for(std::chrono::milliseconds());
    }
    // finalize eCAL API
    eCAL::Finalize();
    return 0;
}

